#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/highgui.h>
#include "opencv2/gpu/gpu.hpp"

image_transport::Publisher image_pub_;

int main(int argc, char** argv) {

    /* initize rosBridge */
    ros::init(argc, argv, "image_converter");
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_(nh_);
    image_transport::Publisher image_pub_;
    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage());
    cv_ptr->encoding = "bgr8";
    image_pub_ = it_.advertise("out", 1);

    // Capture kinect data
    CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY ); //cvCaptureFromFile("/home/youri/GODOT/HOG/test.avi");

    // Initialize variables for frame rate counter
    time_t start, end;
    double fps;
    int counter = 0;
    double sec;

    // If no video stream available exit
    if ( !capture ) {
        fprintf( stderr, "ERROR: capture is NULL \n" );
        getchar();
        return -1;
    }

    // Create video stream window
    cvNamedWindow( "mywindow2", CV_WINDOW_AUTOSIZE );

    // Start tracking time
    time(&start);

    while ( 1 ) {

        // Grab single frame
        IplImage* frame = cvQueryFrame( capture );

        // If no frame exists, then exit
        if ( !frame ) {
            fprintf( stderr, "ERROR: frame is null...\n" );
            getchar();
            cvReleaseCapture( &capture );
            cvDestroyWindow( "mywindow2" );
            return 0;
        }

        // Initialize OpenCV GPU variables
        cv::Mat matFrame(frame,false);
        cv::gpu::GpuMat src_gpu, mono_gpu;

        // Initialize HOG classifier and set descriptor class
        cv::gpu::HOGDescriptor hog;
        hog.setSVMDetector(cv::gpu::HOGDescriptor::getDefaultPeopleDetector());

        // Create vector of bounding boxes
        std::vector<cv::Rect> found;

        // Convert kinect image to OpenCV GPU image
        src_gpu.upload(matFrame);
        cv::gpu::cvtColor(src_gpu, mono_gpu, CV_BGR2GRAY);

        // Detect pedestrains
        hog.detectMultiScale(mono_gpu, found);

        // Draw detected pedestrians in image
        for(unsigned i = 0; i < found.size(); i++) {
            cv::Rect r = found[i];
            rectangle(matFrame, r.tl(), r.br(), cv::Scalar(0,255,0), 2);
        }


        // calculate current FPS
        time(&end);
        ++counter;
        sec = difftime (end, start);
        fps = counter / sec;
        std::cout<< "FPS = " << fps <<std::endl;

        // Show image with detected pedestrians
        cv::imshow("mywindow2", matFrame);

        // Convert OpenCV image to RosBridge message and publish data
        cv_ptr->image = matFrame;
        image_pub_.publish(cv_ptr->toImageMsg());
        ros::spinOnce();

        // exit program if user presses esc
        if ( (cvWaitKey(10) & 255) == 27 ) break;

    }

    // Release the capture device housekeeping
    cvReleaseCapture( &capture );
    cvDestroyWindow( "mywindow2" );
    return 0;

}
